#include "dronecan_driver.h"

// DroneCAN实例和内存池
CanardInstance canard_inst;
uint8_t canard_memory_pool[CANARD_MEMORY_POOL_SIZE];

// CircuitStatus传输ID
static uint8_t circuit_status_transfer_id = 0;

// DroneCAN接收回调函数
static bool should_accept_transfer(const CanardInstance* ins,
                                   uint64_t* out_data_type_signature,
                                   uint16_t data_type_id,
                                   CanardTransferType transfer_type,
                                   uint8_t source_node_id)
{
    (void)ins;
    (void)source_node_id;
    
    // 接受 CircuitStatus 消息
    if (data_type_id == UAVCAN_EQUIPMENT_POWER_CIRCUITSTATUS_ID && 
        transfer_type == CanardTransferTypeBroadcast) {
        *out_data_type_signature = UAVCAN_EQUIPMENT_POWER_CIRCUITSTATUS_SIGNATURE;
        return true;
    }
    
    return false;
}

// DroneCAN传输接收回调
static void on_transfer_reception(CanardInstance* ins, CanardRxTransfer* transfer)
{
    printf("Transfer received - DataTypeID: %d, TransferType: %d, SourceNode: %d\n",
           transfer->data_type_id, transfer->transfer_type, transfer->source_node_id);
    
    // 处理 CircuitStatus 消息
    if (transfer->data_type_id == UAVCAN_EQUIPMENT_POWER_CIRCUITSTATUS_ID) {
        struct uavcan_equipment_power_CircuitStatus circuit_status;
        
        // 使用正确的 decode 函数
        if (uavcan_equipment_power_CircuitStatus_decode(transfer, &circuit_status)) {
            printf("*** CircuitStatus Decoded Successfully ***\n");
            printf("CircuitID: %d, Voltage: %.2f, Current: %.2f, ErrorFlags: 0x%02X\n",
                   circuit_status.circuit_id, circuit_status.voltage, 
                   circuit_status.current, circuit_status.error_flags);
        } else {
            printf("Failed to decode CircuitStatus message\n");
        }
        
        // 释放传输负载
        canardReleaseRxTransferPayload(ins, transfer);
    }
}

// DroneCAN初始化
void canard_init(void)
{
    // 初始化Canard实例
    canardInit(&canard_inst,
               canard_memory_pool,
               sizeof(canard_memory_pool),
               on_transfer_reception,
               should_accept_transfer,
               NULL);
    
    // 设置节点ID
    canardSetLocalNodeID(&canard_inst, DRONECAN_NODE_ID);

    // 调试输出
    printf("DroneCAN Node ID set to: %d\n", canardGetLocalNodeID(&canard_inst));
}

// 处理接收到的CAN帧
void process_canard_rx_frame(const CanardCANFrame* frame)
{
    uint64_t timestamp = HAL_GetTick() * 1000; // 转换为微秒
    canardHandleRxFrame(&canard_inst, frame, timestamp);
}

// 发送CircuitStatus消息
void send_circuit_status(float voltage_data)
{

    printf("Sending CircuitStatus - ID: 0x%03X, Voltage: %.2f\n", UAVCAN_EQUIPMENT_POWER_CIRCUITSTATUS_ID, voltage_data);

    struct uavcan_equipment_power_CircuitStatus circuit_status = {0};
    
    // 填充消息内容
    circuit_status.circuit_id = 1;                    // 电路ID，根据实际情况设置
    circuit_status.voltage = voltage_data;            // 使用ADC数据
    circuit_status.current = 0.0f;                    // 电流，如果没有可以设为0
    circuit_status.error_flags = 0;                   // 错误标志，无错误
    
    // 编码消息
    uint8_t buffer[UAVCAN_EQUIPMENT_POWER_CIRCUITSTATUS_MAX_SIZE];
    uint32_t encoded_size = uavcan_equipment_power_CircuitStatus_encode(&circuit_status, buffer
#if CANARD_ENABLE_TAO_OPTION
        , true
#endif
    );

    // 计算预期的CAN ID
    uint32_t expected_can_id = (2 << 24) | (UAVCAN_EQUIPMENT_POWER_CIRCUITSTATUS_ID << 8) | DRONECAN_NODE_ID;
    printf("Sending CircuitStatus - Expected CAN ID: 0x%08X\n", expected_can_id);
    
    // 发送广播消息
    int16_t result = canardBroadcast(&canard_inst,
                                    UAVCAN_EQUIPMENT_POWER_CIRCUITSTATUS_SIGNATURE,
                                    UAVCAN_EQUIPMENT_POWER_CIRCUITSTATUS_ID,
                                    &circuit_status_transfer_id,
                                    CANARD_TRANSFER_PRIORITY_MEDIUM,
                                    buffer,
                                    encoded_size
#if CANARD_ENABLE_DEADLINE
                                    , 0 // 截止时间
#endif
#if CANARD_MULTI_IFACE
                                    , 0 // 接口掩码
#endif
#if CANARD_ENABLE_CANFD
                                    , false // 不使用CAN FD
#endif
                                    );
    
    printf("canardBroadcast result: %d\n", result);

    // 立即处理TX队列查看实际发送的帧
    const CanardCANFrame* txf = canardPeekTxQueue(&canard_inst);
    while (txf != NULL) {
        printf("TX Queue - CAN ID: 0x%08X, Data Len: %d\n", txf->id, txf->data_len);
        canardPopTxQueue(&canard_inst);
        txf = canardPeekTxQueue(&canard_inst);
    }

    if (result < 0) {
        // 发送失败处理
        // 可以添加错误处理代码
    }
}


